#include "StdAfx.h"
#include "Voxelizer.h"
#include "HashComputer.h"


//#pragma optimize("", off)
//#pragma inline_depth(0)


namespace MNM
{
	Voxelizer::Voxelizer()
		: m_volumeAABB(AABB::RESET)
		, m_voxelConv(ZERO)
		, m_voxelSize(ZERO)
		, m_voxelSpaceSize(ZERO)
	{
	}

	void Voxelizer::Reset()
	{
		m_spanGrid.Reset(0, 0);
	}

	void Voxelizer::Start(const AABB& volume, const Vec3& voxelSize)
	{
		const Vec3 spaceAbs((volume.max - volume.min).abs());
		const Vec3 voxelConv(1.0f / voxelSize.x, 1.0f / voxelSize.y, 1.0f / voxelSize.z);
		m_voxelConv = voxelConv;

		m_voxelSpaceSize((int)((spaceAbs.x * voxelConv.x) + 0.5f), (int)((spaceAbs.y * voxelConv.y) + 0.5f), (int)((spaceAbs.z * voxelConv.z) + 0.5f));
		m_spanGrid.Reset((size_t)m_voxelSpaceSize.x, (size_t)(m_voxelSpaceSize.y));

		m_volumeAABB = volume;
		m_voxelSize = voxelSize;
	}

	void Voxelizer::RasterizeTriangle(const Vec3 v0, const Vec3 v1, const Vec3 v2)
	{
		const Vec3 vertexMin(Minimize(v0, v1, v2));
		const Vec3 vertexMax(Maximize(v0, v1, v2));

		if (!Overlap::AABB_AABB(AABB(vertexMin, vertexMax), m_volumeAABB))
			return;

		const Vec3 e0(v1 - v0);
		const Vec3 e1(v2 - v1);
		const Vec3 e2(v0 - v2);

		const Vec3 n = e2.Cross(e0);

		const bool backface = n.z < 0.0f;

		const Vec3 spaceMin = m_volumeAABB.min;
		const Vec3 voxelSize = m_voxelSize;
		const Vec3 voxelConv = m_voxelConv;
		const Vec3i voxelSpaceSize = m_voxelSpaceSize;

		const Vec3 voxelMin((vertexMin - spaceMin).CompMul(voxelConv));
		const Vec3 voxelMax((vertexMax - spaceMin).CompMul(voxelConv));

		const Vec3i triMin(Maximize<int>(Vec3i((int)voxelMin.x, (int)voxelMin.y, (int)voxelMin.z), Vec3i(0)));
		const Vec3i triMax(Minimize<int>(Vec3i((int)voxelMax.x, (int)voxelMax.y, (int)voxelMax.z), voxelSpaceSize - Vec3(1)));

		const Vec3 dp(voxelSize);

		const Vec3 c(n.x > 0.0f ? dp.x : 0.0f, n.y > 0.0f ? dp.y : 0.0f, n.z > 0.0f ? dp.z : 0.0f);

		const float d1 = n.Dot(c - v0);
		const float d2 = n.Dot(dp - c - v0);

		const bool xycw = n.z < 0.0f;
		const bool xzcw = n.y > 0.0f;
		const bool yzcw = n.x < 0.0f;

		bool zPlanar = triMin.z == triMax.z;

		Vec2 ne0_xy, ne0_xz, ne0_yz;
		float de0_xy, de0_xz, de0_yz;
		Evaluate2DEdge(ne0_xy, de0_xy, xycw, Vec2(e0.x, e0.y), Vec2(v0.x, v0.y), Vec2(dp.x, dp.y));

		if (!zPlanar)
		{
			Evaluate2DEdge(ne0_xz, de0_xz, xzcw, Vec2(e0.x, e0.z), Vec2(v0.x, v0.z), Vec2(dp.x, dp.z));
			Evaluate2DEdge(ne0_yz, de0_yz, yzcw, Vec2(e0.y, e0.z), Vec2(v0.y, v0.z), Vec2(dp.y, dp.z));
		}

		Vec2 ne1_xy, ne1_xz, ne1_yz;
		float de1_xy, de1_xz, de1_yz;
		Evaluate2DEdge(ne1_xy, de1_xy, xycw, Vec2(e1.x, e1.y), Vec2(v1.x, v1.y), Vec2(dp.x, dp.y));

		if (!zPlanar)
		{
			Evaluate2DEdge(ne1_xz, de1_xz, xzcw, Vec2(e1.x, e1.z), Vec2(v1.x, v1.z), Vec2(dp.x, dp.z));
			Evaluate2DEdge(ne1_yz, de1_yz, yzcw, Vec2(e1.y, e1.z), Vec2(v1.y, v1.z), Vec2(dp.y, dp.z));
		}

		Vec2 ne2_xy, ne2_xz, ne2_yz;
		float de2_xy, de2_xz, de2_yz;
		Evaluate2DEdge(ne2_xy, de2_xy, xycw, Vec2(e2.x, e2.y), Vec2(v2.x, v2.y), Vec2(dp.x, dp.y));

		if (!zPlanar)
		{
			Evaluate2DEdge(ne2_xz, de2_xz, xzcw, Vec2(e2.x, e2.z), Vec2(v2.x, v2.z), Vec2(dp.x, dp.z));
			Evaluate2DEdge(ne2_yz, de2_yz, yzcw, Vec2(e2.y, e2.z), Vec2(v2.y, v2.z), Vec2(dp.y, dp.z));
		}

		{
			for (int y = triMin.y; y <= triMax.y; ++y)
			{
				const float minY = spaceMin.y + y * voxelSize.y;

				if ((minY + dp.y < vertexMin.y) || (minY > vertexMax.y))
					continue;

				for (int x = triMin.x; x <= triMax.x; ++x)
				{
					const float minX = spaceMin.x + x * voxelSize.x;

					if ((minX + dp.x < vertexMin.x) || (minX > vertexMax.x))
						continue;

					if (ne0_xy.Dot(Vec2(minX, minY)) + de0_xy < 0.0f)
						continue;
					if (ne1_xy.Dot(Vec2(minX, minY)) + de1_xy < 0.0f)
						continue;
					if (ne2_xy.Dot(Vec2(minX, minY)) + de2_xy < 0.0f)
						continue;

					if (zPlanar)
					{
						m_spanGrid.AddVoxel(x, y, triMin.z, backface);		

						continue;
					}

					for (int z = triMin.z; z <= triMax.z; ++z)
					{
						const float minZ = spaceMin.z + z * voxelSize.z;

						if ((minZ + dp.z < vertexMin.z) || (minZ > vertexMax.z))
							continue;

						float dot = n.Dot(Vec3(minX, minY, minZ));

						if ((dot + d1) * (dot + d2) > 0.0f)
							continue;

						if (ne0_xz.Dot(Vec2(minX, minZ)) + de0_xz < 0.0f)
							continue;
						if (ne1_xz.Dot(Vec2(minX, minZ)) + de1_xz < 0.0f)
							continue;
						if (ne2_xz.Dot(Vec2(minX, minZ)) + de2_xz < 0.0f)
							continue;

						if (ne0_yz.Dot(Vec2(minY, minZ)) + de0_yz < 0.0f)
							continue;
						if (ne1_yz.Dot(Vec2(minY, minZ)) + de1_yz < 0.0f)
							continue;
						if (ne2_yz.Dot(Vec2(minY, minZ)) + de2_yz < 0.0f)
							continue;

						m_spanGrid.AddVoxel(x, y, z, backface);
					}
				}
			}
		}
	}

	static const uint32 BoxTriIndices[] =
	{
		2, 1, 0,
		0, 3, 2,
		3, 0, 7,
		0, 4, 7,
		0, 1, 5, 
		0, 5, 4,
		1, 2, 5,
		6, 5, 2,
		7, 2, 3,
		7, 6, 2,
		7, 4, 5,
		7, 5, 6	
	};

	static inline bool HasNoRotOrScale(const Matrix33& m)
	{
		return ((m.m00 == 1.0f) && (m.m11 == 1.0f) && (m.m22 == 1.0f) &&
			(m.m01 == 0.0f) && (m.m02 == 0.0f) &&
			(m.m10 == 0.0f) && (m.m12 == 0.0f) &&
			(m.m20 == 0.0f) && (m.m21 == 0.0f));
	}

	bool VoxelizeEntity(IPhysicalEntity& physicalEntity, pe_status_dynamics& statusDynamics, pe_status_pos& statusPosition)
	{
		if (physicalEntity.GetType() == PE_WHEELEDVEHICLE)
		{
			return false;
		}

		bool considerMass = (physicalEntity.GetType() == PE_RIGID);
		if (!considerMass)
		{
			if(physicalEntity.GetStatus(&statusPosition))
			{
				considerMass = (statusPosition.iSimClass == SC_ACTIVE_RIGID) || (statusPosition.iSimClass == SC_SLEEPING_RIGID);
			}
		}

		if (considerMass)
		{
			if (!physicalEntity.GetStatus(&statusDynamics))
				return false;

			if (statusDynamics.mass > 1e-6f)
				return false;
		}

		return true;
	}

	size_t WorldVoxelizer::ProcessGeometry(uint32 hashValueSeed, uint32 hashTest, uint32* hashValue)
	{
		size_t triCount = 0;

		const size_t MaxConsideredEntityCount = 2048;
		IPhysicalEntity* entities[MaxConsideredEntityCount] = { 0 };
		IPhysicalEntity** entityList = &entities[0];
		int entityCount = gEnv->pPhysicalWorld->GetEntitiesInBox(m_volumeAABB.min, m_volumeAABB.max, entityList,
			ent_static | ent_terrain | ent_sleeping_rigid | ent_rigid | ent_allocate_list, MaxConsideredEntityCount);

		pe_status_pos sp;
		pe_status_dynamics dyn;

		Matrix34 worldTM;
		sp.pMtx3x4 = &worldTM;

		HashComputer hash(hashValueSeed);
		hash.Add((uint32)entityCount);

		const size_t MaxTerrainAABBCount = 16;
		AABB terrainAABB[MaxTerrainAABBCount];
		size_t terrainAABBCount = 0;

		for (int i = 0; i < entityCount; ++i)
		{
			IPhysicalEntity* entity = entityList[i];

			if (!VoxelizeEntity(*entity, dyn, sp))
				continue;

			sp.ipart = 0;
			MARK_UNUSED sp.partid;

			while (entity->GetStatus(&sp))
			{
				if (sp.pGeomProxy && (sp.flagsOR & geom_colltype_player))
				{
					if (sp.pGeomProxy->GetType() == GEOM_HEIGHTFIELD)
					{
						const AABB aabb = ComputeTerrainAABB(sp.pGeomProxy);
						if (terrainAABBCount < MaxTerrainAABBCount)
							terrainAABB[terrainAABBCount++] = aabb;
						
						if (aabb.GetSize().len2() > 0.0f)
						{
							hash.Add(aabb.min);
							hash.Add(aabb.max);
						}
					}
					else
					{
						hash.Add(sp.BBox[0]);
						hash.Add(sp.BBox[1]);
						hash.Add((uint32)sp.iSimClass);
						hash.Add(worldTM);
					}
				}

				++sp.ipart;
				MARK_UNUSED sp.partid;
			}
			MARK_UNUSED sp.ipart;
		}

		hash.Complete();

		if (hashTest == hash.GetValue())
		{
			if (hashValue)
				*hashValue = hash.GetValue();
			
			if (entities!=entityList)
				gEnv->pPhysicalWorld->GetPhysUtils()->DeletePointer(entityList);

			return 0;
		}

		if (hashValue)
			*hashValue = hash.GetValue();

		terrainAABBCount = 0;

		for (int i = 0; i < entityCount; ++i)
		{
			IPhysicalEntity* entity = entityList[i];

			if(!VoxelizeEntity(*entity, dyn, sp))
				continue;

			sp.ipart = 0;
			MARK_UNUSED sp.partid;

			while (entity->GetStatus(&sp))
			{
				if (sp.pGeomProxy && (sp.flagsOR & geom_colltype_player))
				{
					if (sp.pGeomProxy->GetType() == GEOM_HEIGHTFIELD)
					{
						if (terrainAABBCount < MaxTerrainAABBCount)
						{
							const AABB& aabb = terrainAABB[terrainAABBCount++];

							if (aabb.GetSize().len2() <= 0.0f)
							{
								++sp.ipart;
								MARK_UNUSED sp.partid;

								continue;
							}
						}
					}

					triCount += VoxelizeGeometry(sp.pGeomProxy, worldTM);
				}

				++sp.ipart;
				MARK_UNUSED sp.partid;
			}
			MARK_UNUSED sp.ipart;
		}
		
		if (entities!=entityList)
			gEnv->pPhysicalWorld->GetPhysUtils()->DeletePointer(entityList);

		return triCount;
	}

	struct PhysicsVolume
	{
		PhysicsVolume()
		{
		}

		PhysicsVolume(const IPhysicalEntity* _area, float _volume, const primitives::plane& _plane)
			: area(_area)
			, volume(_volume)
			, plane(_plane)
		{
		}

		bool operator<(const PhysicsVolume& other) const
		{
			return volume < other.volume;
		}

		const IPhysicalEntity* area;
		float volume;
		primitives::plane plane;
	};

	void WorldVoxelizer::CalculateWaterDepth()
	{
		const size_t width = m_spanGrid.GetWidth();
		const size_t height = m_spanGrid.GetHeight();
		const float oceanLevel = gEnv->p3DEngine->GetWaterLevel();

		const size_t MaxAreaCount = 64;
		IPhysicalEntity* areas[MaxAreaCount];

		IPhysicalEntity** areaList = &areas[0];
		size_t areaCount = (size_t)gEnv->pPhysicalWorld->GetEntitiesInBox(m_volumeAABB.min, m_volumeAABB.max, areaList,
			ent_areas | ent_allocate_list, MaxAreaCount);

		if (areaCount > MaxAreaCount)
		{
			assert(areas != &areaList[0]);	
			memcpy(areas, areaList, sizeof(IPhysicalEntity*) * MaxAreaCount);

			gEnv->pPhysicalWorld->GetPhysUtils()->DeletePointer(areaList);

			areaList = areas;
			areaCount = MaxAreaCount;
		}

		const size_t MaxConsiderVolumes = 16;
		
		size_t volumeCount = 0;
		PhysicsVolume volumes[MaxConsiderVolumes];

		pe_params_buoyancy pb;
		pe_params_area pa;

		for(size_t i = 0; i < areaCount; ++i)
		{
			const IPhysicalEntity* area = areaList[i];

			if (area->GetParams(&pb))
			{
				if ((pb.iMedium == 0) && !is_unused(pb.waterPlane.origin))
				{
					if (volumeCount < MaxConsiderVolumes)
					{
						float volume = 0.0f;

						if (area->GetParams(&pa))
						{
							if (pa.pGeom)
								volume = pa.pGeom->GetVolume();
						}

						volumes[volumeCount++] = PhysicsVolume(area, volume, pb.waterPlane);
					}
				}
			}
		}

		const Vec3 spaceMin = m_volumeAABB.min;

		if (!volumeCount && spaceMin.z >= oceanLevel)
			return;

		std::sort(&volumes[0], &volumes[0] + volumeCount);

		const Vec3 voxelSize = m_voxelSize;
		const Vec3 voxelConv = m_voxelConv;

		pe_status_contains_point scp;

		for (size_t y = 0; y < height; ++y)
		{
			for (size_t x = 0; x < width; ++x)
			{
				for (DynamicSpanGrid::Element* span = m_spanGrid[x + y * width]; span ; span = span->next)
				{
					const Vec3 top = spaceMin + Vec3(x * voxelSize.x, y * voxelSize.y, span->top * voxelSize.z);
					size_t depth = (top.z >= oceanLevel) ? 0 : (size_t)((oceanLevel - top.z) * voxelConv.z);

					scp.pt = top;		
					for (size_t i = 0; i < volumeCount; ++i)
					{
						float vdepth = volumes[i].plane.n.z * (volumes[i].plane.n * (volumes[i].plane.origin - top));

						if (volumes[i].area->GetStatus(&scp))
						{
							depth = (vdepth <= 0.0f) ? 0 : (size_t)(vdepth * voxelConv.z);
							break;
						}
					}

					if (depth > DynamicSpanGrid::Element::MaxWaterDepth)
						depth = DynamicSpanGrid::Element::MaxWaterDepth;

					span->depth = depth;
				}
			}
		}
	}

	void WorldVoxelizer::VoxelizeGeometry(const Vec3* vertices, size_t triCount, const Matrix34& worldTM)
	{
		if (HasNoRotOrScale(Matrix33(worldTM)))
		{
			Vec3 offset = worldTM.GetTranslation();

			if (offset.IsZero())
			{
				for (size_t i = 0; i < triCount; ++i)
					RasterizeTriangle(vertices[i * 3 + 0],
					vertices[i * 3 + 1],
					vertices[i * 3 + 2]);
			}
			else
			{
				for (size_t i = 0; i < triCount; ++i)
					RasterizeTriangle(vertices[i * 3 + 0] + offset,
					vertices[i * 3 + 1] + offset,
					vertices[i * 3 + 2] + offset);
			}
		}
		else
		{
			for (size_t i = 0; i < triCount; ++i)
			{
				RasterizeTriangle(worldTM.TransformPoint(vertices[i * 3 + 0]),
					worldTM.TransformPoint(vertices[i * 3 + 1]),
					worldTM.TransformPoint(vertices[i * 3 + 2]));
			}
		}
	}

	void WorldVoxelizer::VoxelizeGeometry(const strided_pointer<Vec3>& vertices, const index_t* indices, size_t triCount, const Matrix34& worldTM)
	{
		if (HasNoRotOrScale(Matrix33(worldTM)))
		{
			Vec3 offset = worldTM.GetTranslation();

			if (offset.IsZero())
			{
				for (size_t i = 0; i < triCount; ++i)
					RasterizeTriangle(vertices[indices[i*3+0]], vertices[indices[i*3+1]], vertices[indices[i*3+2]]);
			}
			else
			{
				for (size_t i = 0; i < triCount; ++i)
				{
					RasterizeTriangle(vertices[indices[i * 3 + 0]] + offset,
						vertices[indices[i * 3 + 1]] + offset,
						vertices[indices[i * 3 + 2]] + offset);
				}
			}
		}
		else
		{
			for (size_t i = 0; i < triCount; ++i)
			{
				RasterizeTriangle(worldTM.TransformPoint(vertices[indices[i * 3 + 0]]),
					worldTM.TransformPoint(vertices[indices[i * 3 + 1]]),
					worldTM.TransformPoint(vertices[indices[i * 3 + 2]]));
			}
		}
	}

	void WorldVoxelizer::VoxelizeGeometry(const Vec3* vertices, const uint32* indices, size_t triCount, const Matrix34& worldTM)
	{
		if (HasNoRotOrScale(Matrix33(worldTM)))
		{
			Vec3 offset = worldTM.GetTranslation();

			if (offset.IsZero())
			{
				for (size_t i = 0; i < triCount; ++i)
					RasterizeTriangle(vertices[indices[i*3+0]], vertices[indices[i*3+1]], vertices[indices[i*3+2]]);
			}
			else
			{
				for (size_t i = 0; i < triCount; ++i)
				{
					RasterizeTriangle(vertices[indices[i * 3 + 0]] + offset,
						vertices[indices[i * 3 + 1]] + offset,
						vertices[indices[i * 3 + 2]] + offset);
				}
			}
		}
		else
		{
			for (size_t i = 0; i < triCount; ++i)
			{
				RasterizeTriangle(worldTM.TransformPoint(vertices[indices[i * 3 + 0]]),
					worldTM.TransformPoint(vertices[indices[i * 3 + 1]]),
					worldTM.TransformPoint(vertices[indices[i * 3 + 2]]));
			}
		}
	}

	AABB WorldVoxelizer::ComputeTerrainAABB(IGeometry* geometry)
	{
		primitives::heightfield* phf = (primitives::heightfield*)geometry->GetData();

		const int minX = max(0, (int)((m_volumeAABB.min.x - phf->origin.x) * phf->stepr.x));
		const int minY = max(0, (int)((m_volumeAABB.min.y - phf->origin.y) * phf->stepr.y));
		const int maxX = min((int)((m_volumeAABB.max.x - phf->origin.x) * phf->stepr.x), (phf->size.x - 1));
		const int maxY = min((int)((m_volumeAABB.max.y - phf->origin.y) * phf->stepr.y), (phf->size.y - 1));

		const Vec3 origin = phf->origin;

		const float xStep = (float)phf->step.x;
		const float yStep = (float)phf->step.y;

		size_t triCount = 0;
		size_t faceCount = 0;

		AABB terrainAABB(AABB::RESET);

		if (phf->fpGetSurfTypeCallback && phf->fpGetHeightCallback)
		{
			for(int y = minY; y <= maxY; ++y)
			{
				for (int x = minX; x <= maxX; ++x)
				{
					if (phf->fpGetSurfTypeCallback(x, y) != phf->typehole)
					{
						const Vec3 v0 = origin + Vec3(x * xStep, y * yStep, phf->getheight(x, y) * phf->heightscale);
						const Vec3 v1 = origin + Vec3(x * xStep, (y + 1) * yStep, phf->getheight(x, y + 1) * phf->heightscale);
						const Vec3 v2 = origin + Vec3((x + 1) * xStep, y * yStep, phf->getheight(x + 1, y) * phf->heightscale);
						const Vec3 v3 = origin + Vec3((x + 1) * xStep, (y + 1) * yStep, phf->getheight(x + 1, y + 1) * phf->heightscale);

						terrainAABB.Add(v0);
						terrainAABB.Add(v1);
						terrainAABB.Add(v2);
						terrainAABB.Add(v3);
					}
				}
			}
		}
		else if (phf->fpGetSurfTypeCallback)
		{
			float* height = (float*)phf->fpGetHeightCallback;

			assert(height);

			for(int y = minY; y <= maxY; ++y)
			{
				for (int x = minX; x <= maxX; ++x)
				{
					const Vec3 v0 = origin + Vec3(x * xStep, y * yStep, height[vector2di(x, y) * phf->stride] * phf->heightscale);
					const Vec3 v1 = origin + Vec3(x * xStep, (y + 1) * yStep, height[vector2di(x, y + 1) * phf->stride] * phf->heightscale);
					const Vec3 v2 = origin + Vec3((x + 1) * xStep, y * yStep, height[vector2di(x + 1, y) * phf->stride] * phf->heightscale);
					const Vec3 v3 = origin + Vec3((x + 1) * xStep, (y + 1) * yStep, height[vector2di(x + 1, y + 1) * phf->stride] * phf->heightscale);

					terrainAABB.Add(v0);
					terrainAABB.Add(v1);
					terrainAABB.Add(v2);
					terrainAABB.Add(v3);
				}
			}
		}
		else
		{
			for(int y = minY; y <= maxY; ++y)
			{
				for (int x = minX; x <= maxX; ++x)
				{
					const Vec3 v0 = origin + Vec3(x * xStep, y * yStep, phf->getheight(x, y) * phf->heightscale);
					const Vec3 v1 = origin + Vec3(x * xStep, (y + 1) * yStep, phf->getheight(x, y + 1) * phf->heightscale);
					const Vec3 v2 = origin + Vec3((x + 1) * xStep, y * yStep, phf->getheight(x + 1, y) * phf->heightscale);
					const Vec3 v3 = origin + Vec3((x + 1) * xStep, (y + 1) * yStep, phf->getheight(x + 1, y + 1) * phf->heightscale);

					terrainAABB.Add(v0);
					terrainAABB.Add(v1);
					terrainAABB.Add(v2);
					terrainAABB.Add(v3);
				}
			}
		}

		if (Overlap::AABB_AABB(m_volumeAABB, terrainAABB))
			return terrainAABB;
		
		return AABB::RESET;
	}

#pragma warning (push)
#pragma warning (disable: 6262)
	size_t WorldVoxelizer::VoxelizeTerrain(IGeometry* geometry, const Matrix34& worldTM)
	{
		primitives::heightfield* phf = (primitives::heightfield*)geometry->GetData();

		const int minX = max(0, (int)((m_volumeAABB.min.x - phf->origin.x) * phf->stepr.x));
		const int minY = max(0, (int)((m_volumeAABB.min.y - phf->origin.y) * phf->stepr.y));
		const int maxX = min((int)((m_volumeAABB.max.x - phf->origin.x) * phf->stepr.x), (phf->size.x - 1));
		const int maxY = min((int)((m_volumeAABB.max.y - phf->origin.y) * phf->stepr.y), (phf->size.y - 1));

		const Vec3 origin = phf->origin;

		const float xStep = (float)phf->step.x;
		const float yStep = (float)phf->step.y;

		size_t triCount = 0;
		size_t faceCount = 0;

		const size_t MaxFaceCount = 1024;
		const size_t MaxVertexCount = 1024 * 4;
		Vec3 vertices[MaxVertexCount];

		if (phf->fpGetSurfTypeCallback && phf->fpGetHeightCallback)
		{
			for(int y = minY; y <= maxY; ++y)
			{
				for (int x = minX; x <= maxX; ++x)
				{
					if (phf->fpGetSurfTypeCallback(x, y) != phf->typehole)
					{
						const Vec3 v0 = origin + Vec3(x * xStep, y * yStep, phf->getheight(x, y) * phf->heightscale);
						const Vec3 v1 = origin + Vec3(x * xStep, (y + 1) * yStep, phf->getheight(x, y + 1) * phf->heightscale);
						const Vec3 v2 = origin + Vec3((x + 1) * xStep, y * yStep, phf->getheight(x + 1, y) * phf->heightscale);
						const Vec3 v3 = origin + Vec3((x + 1) * xStep, (y + 1) * yStep, phf->getheight(x + 1, y + 1) * phf->heightscale);

						assert(faceCount < MaxVertexCount);

						vertices[(faceCount << 2) + 0] = v0;
						vertices[(faceCount << 2) + 1] = v1;
						vertices[(faceCount << 2) + 2] = v2;
						vertices[(faceCount++ << 2) + 3] = v3;
					}
				}
			}
		}
		else if (phf->fpGetSurfTypeCallback)
		{
			float* height = (float*)phf->fpGetHeightCallback;

			assert(height);

			for(int y = minY; y <= maxY; ++y)
			{
				for (int x = minX; x <= maxX; ++x)
				{
					const Vec3 v0 = origin + Vec3(x * xStep, y * yStep, height[vector2di(x, y) * phf->stride] * phf->heightscale);
					const Vec3 v1 = origin + Vec3(x * xStep, (y + 1) * yStep, height[vector2di(x, y + 1) * phf->stride] * phf->heightscale);
					const Vec3 v2 = origin + Vec3((x + 1) * xStep, y * yStep, height[vector2di(x + 1, y) * phf->stride] * phf->heightscale);
					const Vec3 v3 = origin + Vec3((x + 1) * xStep, (y + 1) * yStep, height[vector2di(x + 1, y + 1) * phf->stride] * phf->heightscale);

					assert(faceCount < MaxVertexCount);

					vertices[(faceCount << 2) + 0] = v0;
					vertices[(faceCount << 2) + 1] = v1;
					vertices[(faceCount << 2) + 2] = v2;
					vertices[(faceCount++ << 2) + 3] = v3;
				}
			}
		}
		else
		{
			for(int y = minY; y <= maxY; ++y)
			{
				for (int x = minX; x <= maxX; ++x)
				{
					const Vec3 v0 = origin + Vec3(x * xStep, y * yStep, phf->getheight(x, y) * phf->heightscale);
					const Vec3 v1 = origin + Vec3(x * xStep, (y + 1) * yStep, phf->getheight(x, y + 1) * phf->heightscale);
					const Vec3 v2 = origin + Vec3((x + 1) * xStep, y * yStep, phf->getheight(x + 1, y) * phf->heightscale);
					const Vec3 v3 = origin + Vec3((x + 1) * xStep, (y + 1) * yStep, phf->getheight(x + 1, y + 1) * phf->heightscale);

					assert(faceCount < MaxVertexCount);

					vertices[(faceCount << 2) + 0] = v0;
					vertices[(faceCount << 2) + 1] = v1;
					vertices[(faceCount << 2) + 2] = v2;
					vertices[(faceCount++ << 2) + 3] = v3;
				}
			}
		}

		for (size_t i = 0; i < faceCount; ++i)
		{
			RasterizeTriangle(vertices[(i << 2) + 0], vertices[(i << 2) + 2], vertices[(i << 2) + 1]);
			RasterizeTriangle(vertices[(i << 2) + 1], vertices[(i << 2) + 2], vertices[(i << 2) + 3]);
		}

		return faceCount << 1;
	}
#pragma warning (pop)

#pragma warning (push)
#pragma warning (disable: 6262)
	size_t WorldVoxelizer::VoxelizeGeometry(IGeometry* geometry, const Matrix34& worldTM)
	{
		switch(geometry->GetType())
		{
		case GEOM_TRIMESH:
		case GEOM_VOXELGRID:
			{
				const mesh_data* mesh = static_cast<const mesh_data*>(geometry->GetData());

				VoxelizeGeometry(mesh->pVertices, mesh->pIndices, mesh->nTris, worldTM);

				return mesh->nTris;
			}
			break;
		case GEOM_BOX:
			{
				const primitives::box& box = *static_cast<const primitives::box*>(geometry->GetData());

				Vec3 vertices[] =
				{
					Vec3(-box.size.x, -box.size.y, -box.size.z),
					Vec3( box.size.x, -box.size.y, -box.size.z),
					Vec3( box.size.x,  box.size.y, -box.size.z),
					Vec3(-box.size.x,  box.size.y, -box.size.z),

					Vec3(-box.size.x, -box.size.y,  box.size.z),
					Vec3( box.size.x, -box.size.y,  box.size.z),
					Vec3( box.size.x,  box.size.y,  box.size.z),
					Vec3(-box.size.x,  box.size.y,  box.size.z),
				};

				Matrix34 boxTM(worldTM);

				if (box.bOriented)
				{
					boxTM = Matrix34(box.Basis.GetTransposed());
					boxTM.SetTranslation(box.center);
					boxTM = worldTM * boxTM;
				}
				else
					boxTM.AddTranslation(boxTM.TransformVector(box.center));

				VoxelizeGeometry(vertices, &BoxTriIndices[0], 12, boxTM);

				return 12;
			}
			break;
		case GEOM_SPHERE:
			{
				const primitives::sphere& sphere = *static_cast<const primitives::sphere*>(geometry->GetData());

				const Vec3 center = sphere.center;
				const float r = sphere.r;

				const size_t stacks = 48;
				const size_t slices = 48;

				const size_t vertexCount = slices * (stacks - 2) + 2;
				Vec3 vertices[vertexCount];

				const size_t indexCount = (slices - 1) * (stacks - 2) * 6;
				uint32 indices[indexCount];

				vertices[0] = center;
				vertices[0].y += r;
				vertices[1] = center;
				vertices[1].y -= r;

				size_t v = 2;
				for (size_t j = 1; j < stacks - 1; ++j)
				{
					for (size_t i = 0; i < slices; ++i)
					{
						float theta = (j / (float)(stacks - 1)) * 3.14159265f;
						float phi = (i / (float)(slices - 1)) * 2.0f * 3.14159265f;

						float stheta, ctheta;
						cry_sincosf(theta, &stheta, &ctheta);

						float sphi, cphi;
						cry_sincosf(phi, &sphi, &cphi);

						Vec3 point = center + Vec3(stheta * cphi * r, ctheta * r, -stheta * sphi * r);
						vertices[v++] = point;
					}
				}

				size_t n = 0;

				for (size_t i = 0; i < slices - 1; ++i)  
				{
					indices[n++] = 0;
					indices[n++] = i + 2;
					indices[n++] = i + 3;

					indices[n++] = (stacks - 3) * slices + i + 3;
					indices[n++] = (stacks - 3) * slices + i + 2;
					indices[n++] = 1;
				}

				for (size_t j = 0; j < stacks - 3; ++j)
				{
					for (size_t i = 0; i < slices - 1; ++i)  
					{ 
						indices[n++] = (j + 1) * slices + i + 3;
						indices[n++] = j * slices + i + 3;
						indices[n++] = (j + 1) * slices + i + 2;
						indices[n++] = j * slices + i + 3;
						indices[n++] = j * slices + i + 2;
						indices[n++] = (j + 1) * slices + i + 2;
					}
				}

				VoxelizeGeometry(vertices, indices, indexCount / 3, worldTM);
			}
			break;
		case GEOM_CYLINDER:
			{
				const primitives::cylinder& cylinder = *static_cast<const primitives::cylinder*>(geometry->GetData());

				const Vec3 base = cylinder.center - cylinder.axis * cylinder.hh;
				const Vec3 top = cylinder.center + cylinder.axis * cylinder.hh;

				Vec3 n = cylinder.axis;
				Vec3 a = n.GetOrthogonal();

				Vec3 b = a.Cross(n);
				a = n.Cross(b);

				a.Normalize();
				b.Normalize();

				const size_t slices = 64;
				const float invSlices = 1.0f / (float)slices;

				const float r = cylinder.r;

				const size_t vertexCount = 2 + slices * 4;
				Vec3 vertices[vertexCount];

				const size_t indexCount = slices * 12;
				uint32 indices[indexCount];

				vertices[0] = base;
				vertices[1] = top;

				size_t v = 2;
				for (size_t i = 0; i < slices; ++i)
				{
					float theta0 = i * (3.14159265f * 2.0f * invSlices);
					float theta1 = (i + 1) * (3.14159265f * 2.0f * invSlices);

					float ctheta0, stheta0;
					cry_sincosf(theta0, &stheta0, &ctheta0);
					vertices[v++] = top + a * (r * ctheta0) + b * (r * stheta0);
					vertices[v++] = base + a * (r * ctheta0) + b * (r * stheta0);

					float ctheta1, stheta1;
					cry_sincosf(theta1, &stheta1, &ctheta1);
					vertices[v++] = base + a * (r * ctheta1) + b * (r * stheta1);
					vertices[v++] = top + a * (r * ctheta1) + b * (r * stheta1);
				}

				size_t t = 0;
				for (size_t i = 0; i < slices; ++i)
				{
					indices[t++] = 2 + i * 4 + 2;
					indices[t++] = 2 + i * 4 + 1;
					indices[t++] = 0;

					indices[t++] = 2 + i * 4 + 2;
					indices[t++] = 2 + i * 4 + 1;
					indices[t++] = 2 + i * 4 + 0;

					indices[t++] = 2 + i * 4 + 3;
					indices[t++] = 2 + i * 4 + 2;
					indices[t++] = 2 + i * 4 + 0;

					indices[t++] = 2 + i * 4 + 3;
					indices[t++] = 2 + i * 4 + 0;
					indices[t++] = 1;
				}

				VoxelizeGeometry(vertices, indices, indexCount / 3, worldTM);
			}
			break;
		case GEOM_CAPSULE:
			{
				const primitives::capsule& capsule = *static_cast<const primitives::capsule*>(geometry->GetData());

				const Vec3 base = capsule.center - capsule.axis * capsule.hh;
				const Vec3 top = capsule.center + capsule.axis * capsule.hh;

				Vec3 n = capsule.axis;
				Vec3 a = n.GetOrthogonal();

				Vec3 b = a.Cross(n);
				a = n.Cross(b);

				a.Normalize();
				b.Normalize();
				n.Normalize();

				const size_t stacks = 48;
				const size_t slices = 64;
				const float invSlices = 1.0f / (float)slices;

				const float r = capsule.r;

				{
					const size_t vertexCount = 2 + slices * 4;
					Vec3 vertices[vertexCount];

					const size_t indexCount = slices * 6;
					uint32 indices[indexCount];

					vertices[0] = base;
					vertices[1] = top;

					size_t v = 2;
					for (size_t i = 0; i < slices; ++i)
					{
						float theta0 = i * (3.14159265f * 2.0f * invSlices);
						float theta1 = (i + 1) * (3.14159265f * 2.0f * invSlices);

						float ctheta0, stheta0;
						cry_sincosf(theta0, &stheta0, &ctheta0);
						vertices[v++] = top + a * (r * ctheta0) + b * (r * stheta0);
						vertices[v++] = base + a * (r * ctheta0) + b * (r * stheta0);

						float ctheta1, stheta1;
						cry_sincosf(theta1, &stheta1, &ctheta1);
						vertices[v++] = base + a * (r * ctheta1) + b * (r * stheta1);
						vertices[v++] = top + a * (r * ctheta1) + b * (r * stheta1);
					}

					size_t t = 0;
					for (size_t i = 0; i < slices; ++i)
					{
						indices[t++] = 2 + i * 4 + 2;
						indices[t++] = 2 + i * 4 + 1;
						indices[t++] = 2 + i * 4 + 0;

						indices[t++] = 2 + i * 4 + 3;
						indices[t++] = 2 + i * 4 + 2;
						indices[t++] = 2 + i * 4 + 0;
					}

					VoxelizeGeometry(vertices, indices, indexCount / 3, worldTM);
				}

				{
					// Bottom semi-sphere
					const Vec3 baseCenter = base;
					Vec3 baseDirection = baseCenter - capsule.center;
					baseDirection.Normalize();

					const size_t vertexCount = slices * (stacks - 2) + 2;
					Vec3 vertices[vertexCount];

					const size_t indexCount = (slices - 1) * (stacks - 3) * 6 + 6 * (slices - 1);
					uint32 indices[indexCount];

					vertices[0] = baseCenter + a * r;
					vertices[1] = baseCenter - a * r;

					size_t v = 2;
					for (size_t j = 1; j <= stacks - 2; ++j)
					{
						for (size_t i = 0; i < slices; ++i)
						{
							float theta = (j / (float)(stacks - 1)) * 3.14159265f;
							float phi = (i / (float)(slices - 1)) * 3.14159265f;

							float stheta, ctheta;
							cry_sincosf(theta, &stheta, &ctheta);

							float sphi, cphi;
							cry_sincosf(phi, &sphi, &cphi);

							const Vec3 point = baseCenter + a * ctheta * r + b * stheta * cphi * r + baseDirection * stheta * sphi * r;
							vertices[v++] = point;
						}
					}

					size_t t = 0;
					for (size_t i = 0; i < slices - 1; ++i)
					{
						indices[t++] = i + 2;
						indices[t++] = i + 3;
						indices[t++] = 0;

						indices[t++] = 1;
						indices[t++] = (stacks - 3) * slices + i + 3;
						indices[t++] = (stacks - 3) * slices + i + 2;
					}

					for (size_t j = 0; j < stacks - 3; ++j)
					{
						for (size_t i = 0; i < slices - 1; ++i)
						{ 
							indices[t++] = (j + 1) * slices + i + 3;
							indices[t++] = j * slices + i + 3;
							indices[t++] = (j + 1) * slices + i + 2;

							indices[t++] = j * slices + i + 3;
							indices[t++] = j * slices + i + 2;
							indices[t++] = (j + 1) * slices + i + 2;
						}
					}

					VoxelizeGeometry(vertices, indices, indexCount / 3, worldTM);
				}

				{
					// Top semi-sphere
					const Vec3 topCenter = top;
					Vec3 topDirection = topCenter - capsule.center;
					topDirection.Normalize();

					const size_t vertexCount = slices * (stacks - 2) + 2;
					Vec3 vertices[vertexCount];

					const size_t indexCount = (slices - 1) * (stacks - 3) * 6 + 6 * (slices - 1);
					uint32 indices[indexCount];

					vertices[0] = topCenter + a * r;
					vertices[1] = topCenter - a * r;

					size_t v = 2;
					for (size_t j = 1; j <= stacks - 2; ++j)
					{
						for (size_t i = 0; i < slices; ++i)
						{
							float theta = (j / (float)(stacks - 1)) * 3.14159265f;
							float phi = (i / (float)(slices - 1)) * 3.14159265f;

							float stheta, ctheta;
							cry_sincosf(theta, &stheta, &ctheta);

							float sphi, cphi;
							cry_sincosf(phi, &sphi, &cphi);

							const Vec3 point = topCenter + a * ctheta * r + b * stheta * cphi * r + topDirection * stheta * sphi * r;
							vertices[v++] = point;
						}
					}

					size_t t = 0;
					for (size_t i = 0; i < slices - 1; ++i)
					{ 
						indices[t++] = 0;
						indices[t++] = i + 3;
						indices[t++] = i + 2;

						indices[t++] = (stacks - 3) * slices + i + 2;
						indices[t++] = (stacks - 3) * slices + i + 3;
						indices[t++] = 1;
					}

					for (size_t j = 0; j < stacks - 3; ++j)
					{
						for (size_t i = 0; i < slices - 1; ++i)
						{ 
							indices[t++] = (j + 1) * slices + i + 2;
							indices[t++] = j * slices + i + 3;
							indices[t++] = (j + 1) * slices + i + 3;
							indices[t++] = (j + 1) * slices + i + 2;
							indices[t++] = j * slices + i + 2;
							indices[t++] = j * slices + i + 3;
						}
					}

					VoxelizeGeometry(vertices, indices, indexCount / 3, worldTM);
				}
			}
			break;
		case GEOM_HEIGHTFIELD:
			return VoxelizeTerrain(geometry, worldTM);
			break;
		}

		return 0;
	}
#pragma warning (pop)
}